#include "pcpt_comm/pcpt_comm_format.h"

/**************************************************************************************
功能描述: 初始化
修改记录:
**************************************************************************************/
INT32 PCPT_GRID_C::Init()
{
    if (minX_ >= maxX_ ||
        minY_ >= maxY_ ||
        gridSize_ <= 0) {
        ST_LOG_ERR("Rectangle Params Error, Init Fail.");
        return -1;
    }
    
    gridNumX_ = ceil((maxX_ - minX_) / gridSize_);
    gridNumY_ = ceil((maxY_ - minY_) / gridSize_);
    gridNumXY_ = gridNumX_* gridNumY_;

    isInit_ = true;
    
    return 0;
}

/**************************************************************************************
功能描述: 是否初始化
修改记录:
**************************************************************************************/
BOOL PCPT_GRID_C::IsInit()
{
    return isInit_;
}

/**************************************************************************************
功能描述: 计算栅格坐标
修改记录:
**************************************************************************************/
INT32 PCPT_GRID_C::CalculateGridIndex(const MyPoint &point)
{
    if (!IsInRectangle(point))
        return -1;

    INT32 gridX, gridY;
    CalculateGridCoor(point, gridX, gridY);

    return gridY * gridNumX_ + gridX;
}

INT32 PCPT_GRID_C::CalculateGridIndex(INT32 gridX, INT32 gridY)
{
    if (!IsInRectangle(gridX, gridY))
        return false;

    return gridY * gridNumX_ + gridX;
}

/**************************************************************************************
功能描述: 激光点是否在低矮障碍维护区域内
修改记录:
**************************************************************************************/
BOOL PCPT_GRID_C::IsInRectangle(INT32 gridX, INT32 gridY)
{
    if (gridX < 0 || gridX >= gridNumX_ ||
        gridY < 0 || gridY >= gridNumY_)
        return false;

    return false;
}

BOOL PCPT_GRID_C::IsInRectangle(const MyPoint &point)
{
    if (point.x < minX_ || point.x >= maxX_ ||
        point.y < minY_ || point.y >= maxY_)
        return false;

    return true;
}

/**************************************************************************************
功能描述: 获取栅格数量
修改记录:
**************************************************************************************/
INT32 PCPT_GRID_C::GetGridNumX()
{
    return gridNumX_;
}

INT32 PCPT_GRID_C::GetGridNumY()
{
    return gridNumY_;
}

INT32 PCPT_GRID_C::GetGridNum()
{
    return gridNumXY_;
}

/**************************************************************************************
功能描述: 获取邻近栅格
修改记录:
**************************************************************************************/
void PCPT_GRID_C::GetNeighborGrid(const MyPoint &point, INT32 steps, vector<INT32> &vNeighborIndex)
{
    INT32 gridX, gridY;
    CalculateGridCoor(point, gridX, gridY);
    GetNeighborGrid(gridX, gridY, steps, vNeighborIndex);
}

void PCPT_GRID_C::GetNeighborGrid(INT32 gridX, INT32 gridY, INT32 steps, vector<INT32> &vNeighborIndex)
{
     for(INT32 y = 0 - steps; y <= steps; ++y) {
        INT32 indexY = gridY + y;
        if (indexY < 0 || indexY >= gridNumY_)
            continue;
        
        for(INT32 x = 0 - steps; x <= steps; ++x) {
            INT32 indexX = gridX + x;
            if (indexX < 0 || indexX >= gridNumX_)
                continue;

            if (x == 0 && y == 0)       //Remove self
                continue;

//            if ((x * x + y * y) > (steps + 1) * (steps + 1))
//                continue;

            INT32 neighborIndex = CalculateGridIndex(indexX, indexY);

            vNeighborIndex.push_back(neighborIndex);
        }
    }
}

/**************************************************************************************
功能描述: 计算栅格坐标
修改记录:
**************************************************************************************/
void PCPT_GRID_C::CalculateGridCoor(const MyPoint &point, INT32 &gridX, INT32 &gridY)
{
    gridX = INT32((point.x - minX_) / gridSize_); 
    gridY = INT32((point.y - minY_) / gridSize_);
}

/**************************************************************************************
功能描述: 初始化
修改记录:
**************************************************************************************/
INT32 PCPT_VOXEL_C::Init()
{
    if (minX_ >= maxX_ ||
        minY_ >= maxY_ ||
        minZ_ >= maxZ_ ||
        voxelSize_ <= 0) {
        ST_LOG_ERR("Cuboid Params Error, Init Fail.");
        return -1;
    }
    
    voxelNumX_   = ceil((maxX_ - minX_) / voxelSize_);
    voxelNumY_   = ceil((maxY_ - minY_) / voxelSize_);
    voxelNumZ_   = ceil((maxZ_ - minZ_) / voxelSize_);

    voxelNumXY_  = voxelNumX_ * voxelNumY_;
    voxelNumXYZ_ = voxelNumX_ * voxelNumY_ * voxelNumZ_;

    if (pVoxelArray_ == NULL) {
        pVoxelArray_ = new UINT08[voxelNumXYZ_];
        isInit_ = true;
        return 0;
    } else {
        delete[] pVoxelArray_;
        pVoxelArray_ = NULL;
        return -1;
    }
}

/**************************************************************************************
功能描述: 是否初始化
修改记录:
**************************************************************************************/
BOOL PCPT_VOXEL_C::IsInit()
{
    return isInit_;
}

/**************************************************************************************
功能描述: 复位体素数组
修改记录:
**************************************************************************************/
void PCPT_VOXEL_C::ResetVoxelArray()
{
    memset(pVoxelArray_, 0, voxelNumXYZ_);
}

INT32 PCPT_VOXEL_C::AddPoint2VoxelCellWithCoor(const MyPoint &point, VOXEL_COORDINATE_STRU &stVoxelCoor)
{
    if (!IsInVoxelCuboid(point))
        return -1;
    
    CalculateVoxelCoor(point, stVoxelCoor);

    Add2VoxelCell(stVoxelCoor);

    return 0;
}

/**************************************************************************************
功能描述: 邻近体素数量
修改记录:
**************************************************************************************/
INT32 PCPT_VOXEL_C::NeighborVoxelCount(const VOXEL_COORDINATE_STRU &stVoxelCoor, INT32 steps)
{
    vector<INT32> vNeighborIndex;
    vNeighborIndex.reserve((2 * steps + 1) * 9);
    
    GetNeighborVoxel(stVoxelCoor, steps, vNeighborIndex);
    
    return vNeighborIndex.size();
}

/**************************************************************************************
功能描述: 所有邻近体素的值的和
修改记录:
**************************************************************************************/
INT32 PCPT_VOXEL_C::NeighborVoxelValue(const VOXEL_COORDINATE_STRU &stVoxelCoor, INT32 steps)
{
    vector<INT32> vNeighborIndex;
    vNeighborIndex.reserve((2 * steps + 1) * 9);
    
    GetNeighborVoxel(stVoxelCoor, steps, vNeighborIndex);
    
    INT32 total = 0;
    for (const INT32 &value : vNeighborIndex)
        total += value;

    return total;
}

/**************************************************************************************
功能描述: 有足够的邻近体素？
修改记录:
**************************************************************************************/
BOOL PCPT_VOXEL_C::HasEnoughNeigVoxelCount(const VOXEL_COORDINATE_STRU &stVoxelCoor, INT32 steps, INT32 minNeighbors)
{
    INT32 neighborIndex = 0;
    INT32 neighborCount = 0;
    
    for (INT32 z = 0 - steps; z <= steps; ++z) {
        INT32 indexZ = stVoxelCoor.z + z;
        if (indexZ < 0 || indexZ >= voxelNumZ_)
            continue;
        
        for(INT32 y = 0 - steps; y <= steps; ++y) {
            INT32 indexY = stVoxelCoor.y + y;
            if (indexY < 0 || indexY >= voxelNumY_)
                continue;
            
            for(INT32 x = 0 - steps; x <= steps; ++x) {
                INT32 indexX = stVoxelCoor.x + x;
                if (indexX < 0 || indexX >= voxelNumX_)
                    continue;

                if (x == 0 && y == 0 && z == 0)     //Remove self
                    continue;

//                if ((x * x + y * y + z * z) > (steps + 1) * (steps + 1))
//                    continue;

                neighborIndex = CalculateVoxelIndex(indexX, indexY, indexZ);

                if (pVoxelArray_[neighborIndex]) {
                    neighborCount++;
                    if (neighborCount >= minNeighbors)
                        return true;
                }
            }
        }
    }

    return false;
}

/**************************************************************************************
功能描述: 有足够的邻近体素值的和？
修改记录:
**************************************************************************************/
BOOL PCPT_VOXEL_C::HasEnoughNeigVoxelValue(const VOXEL_COORDINATE_STRU &stVoxelCoor, INT32 steps, INT32 minNeighbors)
{
    INT32 neighborIndex = 0;
    INT32 neighborCount = 0;
    
    for (INT32 z = 0 - steps; z <= steps; ++z) {
        INT32 indexZ = stVoxelCoor.z + z;
        if (indexZ < 0 || indexZ >= voxelNumZ_)
            continue;
        
        for(INT32 y = 0 - steps; y <= steps; ++y) {
            INT32 indexY = stVoxelCoor.y + y;
            if (indexY < 0 || indexY >= voxelNumY_)
                continue;
            
            for(INT32 x = 0 - steps; x <= steps; ++x) {
                INT32 indexX = stVoxelCoor.x + x;
                if (indexX < 0 || indexX >= voxelNumX_)
                    continue;

                if (x == 0 && y == 0 && z == 0)     //Remove self
                    continue;

                if ((x * x + y * y + z * z) > (steps + 1) * (steps + 1))
                    continue;

                neighborIndex = CalculateVoxelIndex(indexX, indexY, indexZ);

                if (pVoxelArray_[neighborIndex]) {
                    neighborCount += pVoxelArray_[neighborIndex];
                    if (neighborCount >= minNeighbors)
                        return true;
                }
            }
        }
    }
    return false;
}

/**************************************************************************************
功能描述: 获取体素数量
修改记录:
**************************************************************************************/
INT32 PCPT_VOXEL_C::GetVoxelNumX()
{
    return voxelNumX_;
}

INT32 PCPT_VOXEL_C::GetVoxelNumY()
{
    return voxelNumY_;
}

INT32 PCPT_VOXEL_C::GetVoxelNumZ()
{
    return voxelNumZ_;
}

INT32 PCPT_VOXEL_C::GetVoxelNumXY()
{
    return voxelNumXY_;
}

INT32 PCPT_VOXEL_C::GetVoxelNum()
{
    return voxelNumXYZ_;
}

/**************************************************************************************
功能描述: 是否在体素空间内
修改记录:
**************************************************************************************/
BOOL PCPT_VOXEL_C::IsInVoxelCuboid(const VOXEL_COORDINATE_STRU &stVoxelCoor)
{
    return IsInVoxelCuboid(stVoxelCoor.x, stVoxelCoor.y, stVoxelCoor.z);
}

BOOL PCPT_VOXEL_C::IsInVoxelCuboid(INT32 x, INT32 y, INT32 z)
{
    if (x < 0 || x >= voxelNumX_ ||
        y < 0 || y >= voxelNumY_ ||
        z < 0 || z >= voxelNumZ_ )
        return false;

    return true;
}

BOOL PCPT_VOXEL_C::IsInVoxelCuboid(const MyPoint &point)
{
    if (point.x < minX_ || point.x >= maxX_ ||
        point.y < minX_ || point.y >= maxX_ ||
        point.z < minX_ || point.z >= maxX_ )
        return false;

    return true;
}

/**************************************************************************************
功能描述: 计算体素坐标
修改记录:
**************************************************************************************/
void PCPT_VOXEL_C::CalculateVoxelCoor(const MyPoint &point, VOXEL_COORDINATE_STRU &stVoxelCoor)
{
    stVoxelCoor.x = INT32((point.x - minX_) / voxelSize_);
    stVoxelCoor.y = INT32((point.y - minY_) / voxelSize_);
    stVoxelCoor.z = INT32((point.z - minZ_) / voxelSize_);
}

/**************************************************************************************
功能描述: 获取体素索引
修改记录:
**************************************************************************************/
INT32 PCPT_VOXEL_C::CalculateVoxelIndex(const VOXEL_COORDINATE_STRU &stVoxelCoor)
{
    return CalculateVoxelIndex(stVoxelCoor.x, stVoxelCoor.y, stVoxelCoor.z);
}

INT32 PCPT_VOXEL_C::CalculateVoxelIndex(INT32 x, INT32 y, INT32 z)
{
    return z * voxelNumXY_ + y * voxelNumX_ + x;
}

/**************************************************************************************
功能描述: 累加体素索引对应的值
修改记录:
**************************************************************************************/
void PCPT_VOXEL_C::Add2VoxelCell(const VOXEL_COORDINATE_STRU &stVoxelCoor)
{
    INT32 index = CalculateVoxelIndex(stVoxelCoor.x, stVoxelCoor.y, stVoxelCoor.z);
    pVoxelArray_[index] += 1;
}

/**************************************************************************************
功能描述: 获取所有邻近体素
修改记录:
**************************************************************************************/
void PCPT_VOXEL_C::GetNeighborVoxel(const VOXEL_COORDINATE_STRU &stVoxelCoor, INT32 steps, vector<INT32> &vNeighborIndex)
{
    for(INT32 z = 0 - steps; z <= steps; ++z) {
        INT32 indexZ = stVoxelCoor.z + z;
        if (indexZ < 0 || indexZ >= voxelNumZ_)
            continue;
        
        for(INT32 y = 0 - steps; y <= steps; ++y) {
            INT32 indexY = stVoxelCoor.y + y;
            if (indexY < 0 || indexY >= voxelNumY_)
                continue;
            
            for(INT32 x = 0 - steps; x <= steps; ++x) {
                INT32 indexX = stVoxelCoor.x + x;
                if (indexX < 0 || indexX >= voxelNumX_)
                    continue;

                if (x == 0 && y == 0 && z == 0)     //Remove self
                    continue;

//                if ((x * x + y * y + z * z) > (steps + 1) * (steps + 1))
//                    continue;

                INT32 neighborIndex = CalculateVoxelIndex(indexX, indexY, indexZ);
                
                if (pVoxelArray_[neighborIndex])
                    vNeighborIndex.push_back(neighborIndex);
            }
        }
    }
}

